#include "board_com.h"
#include "RGB.h"
#include "beep.h"
#include "can_common.h"
#include "offline.h"
#include "robotdef.h"
#include "rtthread.h"
#include "rttypes.h"

extern rt_device_t can1_bus;  /* CAN 设备句柄 */
extern rt_device_t can2_bus;


#define LOG_TAG              "board_com"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>

void board_com_process(struct rt_can_msg rxmsg);
uint8_t float_to_uint8(float f);
float uint8_to_float(uint8_t u);

board_com_s board_com;
int board_com_init(void)
{
    // 初始化板间通讯的掉线检测
    offline_manage_obj board;
    board.enable = OFFLINE_ENABLE;
    board.online_state = STATE_OFFLINE;
    board.error_level = OFFLINE_ERROR_LEVEL;
    board.last_time = rt_tick_get();
    board.offline_time = 500;

    //板间通讯部分常量定义
    board_com.board_tx.txmsg.ide =RT_CAN_STDID;
    board_com.board_tx.txmsg.rtr =RT_CAN_DTR;
    board_com.board_tx.txmsg.len =8;

    
#ifndef ONE_BOARD

    #if defined(HERO_MODE) || defined(ENGINEER_MODE) || defined (INFANTRY_MODE)
        #ifdef GIMBAL_BOARD
            board.event = OFFLINE_BOARD_CHASSIS;
            board.error_level = OFFLINE_ERROR_LEVEL;
            board.beep_times = 6;
            offline_event_init(board);
        #endif 

        #ifdef CHASSIS_BOARD
            board.event = OFFLINE_BOARD_BIG_YAW;
            board.error_level = OFFLINE_ERROR_LEVEL;
            board.beep_times = 6;
            offline_event_init(board);
        #endif 
    #endif 

    #if defined (SENTRY_MODE)

        #ifdef SENTRY_BOARD_SMALL_YAW_1
            board.event = OFFLINE_BOARD_BIG_YAW;
            board.error_level = OFFLINE_ERROR_LEVEL;
            board.beep_times = 6;
            offline_event_init(board);

        #endif 
        #ifdef SENTRY_BOARD_SMALL_YAW_2
            board.event = OFFLINE_BOARD_BIG_YAW;
            board.error_level = OFFLINE_ERROR_LEVEL;
            board.beep_times = 6;
            offline_event_init(board);

        #endif 
        #ifdef SENTRY_BOARD_BIG_YAW
            board.event = OFFLINE_BOARD_SMALL_YAW_1;
            board.error_level = OFFLINE_WARNING_LEVEL;
            board.beep_times = 6;
            offline_event_init(board);


            board.event = OFFLINE_BOARD_SMALL_YAW_2;
            board.error_level = OFFLINE_WARNING_LEVEL;
            board.beep_times = 6;
            offline_event_init(board);


            board.event = OFFLINE_BOARD_CHASSIS;
            board.error_level = OFFLINE_WARNING_LEVEL;
            board.beep_times = 6;
            offline_event_init(board);

        #endif 

        #ifdef SENTRY_BOARD_CHASSIS
            board.event = OFFLINE_BOARD_BIG_YAW;
            board.error_level = OFFLINE_ERROR_LEVEL;
            board.beep_times = 6;
            offline_event_init(board);
        #endif 

    #endif 

#endif 

    return RT_EOK;
}
INIT_APP_EXPORT(board_com_init);

void board_recv(struct rt_can_msg rxmsg)
{

#ifndef ONE_BOARD

    #if defined(HERO_MODE) || defined(ENGINEER_MODE) || defined (INFANTRY_MODE)
        #ifdef GIMBAL_BOARD
            //防止烧错代码，出现多个相同设备的情况
            if (rxmsg.id == GIMBAL_ID)
            {
                aRGB_led_show(LED_White);
                beep_set_tune(1000000,300000);
                LOG_W("common device find,please check board define\n");
                RT_ASSERT(0);
            }
            if (rxmsg.id == CHASSIS_ID)
            {
                offline_event_time_update(OFFLINE_BOARD_CHASSIS);
                board_com_process(rxmsg);
            }
        #endif 

        #ifdef CHASSIS_BOARD
            //防止烧错代码，出现多个相同设备的情况
            if (rxmsg.id == CHASSIS_ID)
            {
                aRGB_led_show(LED_White);
                beep_set_tune(1000000,300000);
                RT_ASSERT(0);
            }
            if (rxmsg.id == GIMBAL_ID)
            {
                offline_event_time_update(OFFLINE_BOARD_BIG_YAW);
                board_com_process(rxmsg);
            }
        #endif 
    #endif  

    #if defined (SENTRY_MODE)
        #ifdef SENTRY_BOARD_SMALL_YAW_1
            //防止烧错代码，出现多个相同设备的情况
            if (rxmsg.id == BOARD_SMALL_YAW_1_ID)
            {
                aRGB_led_show(LED_White);
                beep_set_tune(1000000,300000);
                RT_ASSERT(0);
            }
            board_com_process(rxmsg);

        #endif 
        #ifdef SENTRY_BOARD_SMALL_YAW_2
            //防止烧错代码，出现多个相同设备的情况
            if (rxmsg.id == BOARD_SMALL_YAW_2_ID)
            {
                aRGB_led_show(LED_White);
                beep_set_tune(1000000,300000);
                RT_ASSERT(0);
            }
            board_com_process(rxmsg);
        #endif 
        #ifdef SENTRY_BOARD_BIG_YAW
            //防止烧错代码，出现多个相同设备的情况
            if (rxmsg.id == BOARD_BIG_YAW_ID)
            {
                aRGB_led_show(LED_White);
                beep_set_tune(1000000,300000);
                LOG_W("common device find,please check board define\n");
                RT_ASSERT(0);
            }
            board_com_process(rxmsg);
        #endif 

        #ifdef SENTRY_BOARD_CHASSIS
            //防止烧错代码，出现多个相同设备的情况
            if (rxmsg.id == BOARD_CHASSIS_ID)
            {
                aRGB_led_show(LED_White);
                beep_set_tune(1000000,300000);
                RT_ASSERT(0);
            }
            board_com_process(rxmsg);
        #endif 
    #endif

#endif     
    
}

void *CANCommGet(rt_uint8_t checkid)
{
    #if defined(HERO_MODE) || defined(ENGINEER_MODE) || defined (INFANTRY_MODE)
    #ifdef CHASSIS_BOARD
    return &board_com.Chassis_Ctrl_Cmd;
    #endif
    #endif

    #if defined(SENTRY_MODE)
    #ifdef SENTRY_BOARD_CHASSIS
    return &board_com.Chassis_Ctrl_Cmd;
    #endif

    #ifdef SENTRY_BOARD_BIG_YAW
    return &board_com.Chassis_Upload_Data;
    #endif

    #if defined (SENTRY_BOARD_SMALL_YAW_1) || defined (SENTRY_BOARD_SMALL_YAW_2) 
    if (checkid == GIMBAL_TAIL_CHECK_ID)
    {
        return &board_com.Gimbal_Ctrl_Cmd;
    }
    else if (checkid == SHOOT_TAIL_CHECK_ID)
    {
        return &board_com.Shoot_Ctrl_Cmd;
    }
    #endif

    #endif

    return RT_NULL; // 默认返回 NULL
}

/*
   target_id 发送者的id,checkid 对于gimbal/small yaw/chaais 是用来区分云台指令和发送指令和底盘指令
*/
void board_send(rt_uint32_t target_id, void *data, rt_uint8_t checkid)
{
    rt_uint8_t tmpdata[7]={0};
    switch (checkid) 
    {
        case GIMBAL_TAIL_CHECK_ID:
            {
                Gimbal_Ctrl_Cmd_s *cmd = RT_NULL; 
                cmd = (Gimbal_Ctrl_Cmd_s *)data; 
                tmpdata[0] = ((rt_int16_t)(cmd->yaw              *100) >> 8) & 0xFF;      
                tmpdata[1] = ((rt_int16_t)(cmd->yaw               *100) & 0xFF);            
                tmpdata[2] = ((rt_int16_t)(cmd->pitch           *100) >> 8) & 0xFF;       
                tmpdata[3] = ((rt_int16_t)(cmd->pitch            *100) & 0xFF);                
                tmpdata[4] = cmd->gimbal_mode;
                break;
            }
        case SHOOT_TAIL_CHECK_ID:
            {
                Shoot_Ctrl_Cmd_s *cmd = RT_NULL; 
                cmd = (Shoot_Ctrl_Cmd_s *)data; 
                tmpdata[0] = cmd->shoot_mode;   
                tmpdata[1] = cmd->friction_mode;            
                tmpdata[2] = cmd->load_mode;   
                tmpdata[3] = cmd->shoot_rate;            
                tmpdata[4] = (cmd->rest_heat_1)/4;   
                tmpdata[5] = (cmd->rest_heat_1)/4; 
                break;
            }
        case CHASSIS_TAIL_CHECK_ID:
            {
                Chassis_Ctrl_Cmd_s *cmd = RT_NULL; 
                cmd = (Chassis_Ctrl_Cmd_s *)data; 
                tmpdata[0] = ((rt_int16_t)(cmd->vx           *100) >> 8) & 0xFF;      
                tmpdata[1] = ((rt_int16_t)(cmd->vx           *100) & 0xFF);            
                tmpdata[2] = ((rt_int16_t)(cmd->vy           *100) >> 8) & 0xFF;       
                tmpdata[3] = ((rt_int16_t)(cmd->vy           *100) & 0xFF);             
                tmpdata[4] = ((rt_int16_t)(cmd->offset_angle *100) >> 8) & 0xFF;   
                tmpdata[5] = ((rt_int16_t)(cmd->offset_angle *100) & 0xFF);    
                tmpdata[6] = cmd->chassis_mode;
                break;
            }
        case SMALL_YAW_1_HEART_CHECK_ID:
        case SMALL_YAW_2_HEART_CHECK_ID:
            {
                Small_Yaw_Heart_Pack_s *cmd = RT_NULL; 
                cmd = (Small_Yaw_Heart_Pack_s *)data; 
                tmpdata[0] = cmd->enemy_found;      
                break;
            }
        case CHASSIS_UPLOAD_DATA_CHECK_ID:
            {
                Chassis_Upload_Data_s *cmd = RT_NULL; 
                cmd = (Chassis_Upload_Data_s *)data; 
                uint8_t combined = 0;
                combined |= (cmd->power_management_gimbal_output & 0x01) << 0;
                combined |= (cmd->power_management_chassis_output & 0x01) << 1;
                combined |= (cmd->power_management_shooter_output & 0x01) << 2;
                combined |= (cmd->game_progress & 0x01) << 3;
                combined |= (cmd->event_supply_area_rmuc & 0x01) << 4;
                combined |= (cmd->event_supply_area_rmul & 0x01) << 5;
                combined |= (cmd->event_center_aera_rmul & 0x01) << 6;
            
                tmpdata[0] = (cmd->current_hp_percent / 4); // 0-400 -> 0-100
                tmpdata[1] = combined;
                tmpdata[2] = ((rt_int16_t)cmd->shooter_17mm_1_barrel_heat >> 8) & 0xFF; 
                tmpdata[3] = ((rt_int16_t)cmd->shooter_17mm_1_barrel_heat & 0xFF);  
                tmpdata[4] = ((rt_int16_t)cmd->shooter_17mm_2_barrel_heat >> 8) & 0xFF; 
                tmpdata[5] = ((rt_int16_t)cmd->shooter_17mm_2_barrel_heat & 0xFF);
                tmpdata[6] = 0;
                break;
            }
    }
    board_com.board_tx.txmsg.id = target_id;
    board_com.board_tx.txmsg.data[7] = checkid;
    rt_memcpy(&board_com.board_tx.txmsg.data,tmpdata,sizeof(tmpdata));
    if (*board_com.board_tx.can_handle != RT_NULL) {
        rt_device_write(*board_com.board_tx.can_handle,0,&board_com.board_tx.txmsg,sizeof(board_com.board_tx.txmsg));
    }
    else {return;}
}

void board_com_process(struct rt_can_msg rxmsg){
#ifndef ONE_BOARD

    #if defined(HERO_MODE) || defined(ENGINEER_MODE) || defined (INFANTRY_MODE)
        #ifdef GIMBAL_BOARD

        #endif 
        #ifdef CHASSIS_BOARD

        #endif 
    #endif  

    #if defined (SENTRY_MODE)
        #if defined (SENTRY_BOARD_SMALL_YAW_1) || defined (SENTRY_BOARD_SMALL_YAW_2) 
            if (rxmsg.id == BOARD_BIG_YAW_ID)
            {
                if (rxmsg.data[7]  == GIMBAL_TAIL_CHECK_ID)
                {
                    offline_event_time_update(OFFLINE_BOARD_BIG_YAW);
                    board_com.Gimbal_Ctrl_Cmd.yaw = ((int16_t)(rxmsg.data[0] << 8) | rxmsg.data[1]) / 100.0f;
                    board_com.Gimbal_Ctrl_Cmd.pitch = ((int16_t)(rxmsg.data[2] << 8) | rxmsg.data[3]) / 100.0f;
                    board_com.Gimbal_Ctrl_Cmd.gimbal_mode  = rxmsg.data[4];
                }
                else if (rxmsg.data[7] == SHOOT_TAIL_CHECK_ID)
                {
                    offline_event_time_update(OFFLINE_BOARD_BIG_YAW);
                    board_com.Shoot_Ctrl_Cmd.shoot_mode = rxmsg.data[0];
                    board_com.Shoot_Ctrl_Cmd.friction_mode = rxmsg.data[1];
                    board_com.Shoot_Ctrl_Cmd.load_mode = rxmsg.data[2];
                    board_com.Shoot_Ctrl_Cmd.shoot_rate = rxmsg.data[3];
                    board_com.Shoot_Ctrl_Cmd.rest_heat_1 = rxmsg.data[4]*4;               
                    board_com.Shoot_Ctrl_Cmd.rest_heat_2 = rxmsg.data[5]*4;        
                }
            }
        #endif 
        #ifdef SENTRY_BOARD_BIG_YAW
            switch (rxmsg.id) 
            {
                case BOARD_CHASSIS_ID:
                {
                    offline_event_time_update(OFFLINE_BOARD_CHASSIS);
                    board_com.Chassis_Upload_Data.current_hp_percent = rxmsg.data[0] *4;
                    uint8_t combined = rxmsg.data[1];
                    board_com.Chassis_Upload_Data.power_management_gimbal_output = (combined >> 0) & 0x01;
                    board_com.Chassis_Upload_Data.power_management_chassis_output = (combined >> 1) & 0x01;
                    board_com.Chassis_Upload_Data.power_management_shooter_output = (combined >> 2) & 0x01;
                    board_com.Chassis_Upload_Data.game_progress = (combined >> 3) & 0x01;
                    board_com.Chassis_Upload_Data.event_supply_area_rmuc = (combined >> 4) & 0x01;
                    board_com.Chassis_Upload_Data.event_supply_area_rmul = (combined >> 5) & 0x01;
                    board_com.Chassis_Upload_Data.event_center_aera_rmul = (combined >> 6) & 0x01;
                    board_com.Chassis_Upload_Data.shooter_17mm_1_barrel_heat = ((int16_t)(rxmsg.data[2] << 8) | rxmsg.data[3]);
                    board_com.Chassis_Upload_Data.shooter_17mm_2_barrel_heat = ((int16_t)(rxmsg.data[4] << 8) | rxmsg.data[5]);
                    break;
                }
                case BOARD_SMALL_YAW_1_ID:
                {
                    offline_event_time_update(OFFLINE_BOARD_SMALL_YAW_1);
                    board_com.Small_Yaw_Heart_Pack_1.enemy_found = rxmsg.data[0];
                    break;
                }
                case BOARD_SMALL_YAW_2_ID:
                {
                    offline_event_time_update(OFFLINE_BOARD_SMALL_YAW_2);
                    board_com.Small_Yaw_Heart_Pack_2.enemy_found = rxmsg.data[0];
                    break;
                }
            }
        #endif 

        #ifdef SENTRY_BOARD_CHASSIS
            if (rxmsg.id == BOARD_BIG_YAW_ID)
            {
                if (rxmsg.data[7] == CHASSIS_TAIL_CHECK_ID)
                {
                    offline_event_time_update(OFFLINE_BOARD_BIG_YAW);
                    board_com.Chassis_Ctrl_Cmd.vx = ((int16_t)(rxmsg.data[0] << 8) | rxmsg.data[1]) /100.0f;
                    board_com.Chassis_Ctrl_Cmd.vy = ((int16_t)(rxmsg.data[2] << 8) | rxmsg.data[3]) /100.0f;
                    board_com.Chassis_Ctrl_Cmd.offset_angle = ((int16_t)(rxmsg.data[4] << 8) | rxmsg.data[5]) / 100.0f;
                    board_com.Chassis_Ctrl_Cmd.chassis_mode = rxmsg.data[6];
                }
            }
        #endif 
    #endif

#endif     
}

/*私有函数*/
uint8_t float_to_uint8(float f) {
    // 确保输入在 -2.0 到 2.0 之间
    if (f < -2.0f) f = -2.0f;
    if (f > 2.0f) f = 2.0f;

    // 缩放和偏移
    uint8_t u = (uint8_t)((f + 2.0f) * (255.0f / 4.0f));
    return u;
}

float uint8_to_float(uint8_t u) {
    // 反向转换
    float f = (u * (4.0f / 255.0f)) - 2.0f;
    return f;
}





